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(57) Abstract: An image display system and method 
that use physical models to produce a realistic display 
of a scene are disclosed. The image display method 
involves operating a computer having a display screen, 
a memory and a processing unit for simulating the 
motion of objects and displaying the results on the 
display screen. The method includes the steps of 
storing in the memory position and velocity parameters 
which define an initial state of a model system 
having a plurality of bodies, storing in the memory 
parameters which define at least one constraint function 
constraining the motion of the bodies in the model 
system, and calculating in the processor the position 
and velocity parameters which define the state of the 
system after a predetermined time step based on rigid 
body dynamics, including carrying out a semi-implicit 
integration step subject to the constraints, to determine 
the velocity after the step, including determining 
the constraint forces that act to keep the system in 
compliance with the constraints by ensuring that the 
first derivative of the constraint function is zero. 
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IMAGE DISPLAY APPARATUS , METHOD AND PROGRAM BASED ON RIGID BODY DYNAMICS 

5 The invention relates to an image display system and 

method, and in particular to an image display system that uses 
physical dynamic models to produce a realistic display of a 
scene . 

It is becoming increasingly useful to display scenes on 
10 computer displays that represent the real world. Such scenes 
may occur in virtual reality devices, simulators and computer 
games . 

One way of providing such scenes is to film images and to 
display the recorded images on the display. However, this 

15 approach requires that the content of the scene is 

predetermined and appropriate film sequences created and pre- 
stored in the computer. Thus, such an approach cannot be used 
where the scenes are not wholly scripted, which makes the 
approach unsuitable for simulations and computer games in which 

20 the user can carry out actions not predicted by the programmer. 
An alternative approach uses a simulation of rigid body 
dynamics to allow scenes including such objects to be displayed 
realistically. In order to cope with simulation applications, 
the model has to be able to cope with a variable number of 

25 simulated objects that can be created and destroyed. 
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Such models should model a plurality of rigid objects that 
can interact with each other, subject to constraints. For 
example, if one object is hinged to another object that hinge 
acts as a constraint; the two objects cannot move 
5 independently. The existence of constraints makes the problem 
much more difficult to solve than a simple application of 
Newton's laws of motion. 

A number of prior approaches have been presented but these 
have not proven wholly successful. The most suitable for 
) simulation of multiple objects are so-called "extended 

coordinate methods- in which the constraints are introduced 
using Lagrange multipliers that correspond to forces that 
maintain the constraints. However, there are difficulties with 
these approaches. 

Firstly, the known methods use a large number of 
variables, using nearly doubling the number of variables 
(because of the Lagrange multipliers) to describe the system, 
which results in them being eight times more computationally 
intensive than an equivalent system without constraints. Thus, 
the prior art methods tend to be highly inefficient. 

Secondly, the known methods use differential algebraic 
equations that are numerically rather stiff. Simple methods 
for solving such equations are rather unstable. 

Thirdly, it is not known how to efficiently incorporate 
friction into such systems. As will be appreciated, friction 
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is an important property of real physical systems that has to 
be modelled correctly for a realistic result. This is a 
difficult problem but a working solution was reported in D.E. 
Stewart and J.C. Trinkle, "An implicit time-stepping scheme for 
rigid body dynamics with inelastic collisions and coulomb 
friction" , International for numerical methods in engineering, 
volume 39 pages 2673-2691 (1996), and was improved on by Mihai 
Anitescu and F.A. Potra, "Formulating dynamic multi-rigid-body 
contact problems with friction as solvable linear 
complementarity problems " , Non-linear Dynamics, volume 14 pages 
231-237 (1997) . The approach described allows consistent 
models in which the velocities can always be computed and are 
always finite. The disadvantage of the approach is that the 
model involves solving a particular class of linear 
complementarity problem which has a structure such that not all 
algorithms are suitable. Anitescu and Trinkle used the Lemke 
algorithm but this is inefficient and prone to large errors. 

A fourth difficulty with prior art approaches is that the 
constraints are generated automatically; such constraints need 
not be not independent of one another which results in the 
system being degenerate. Geometric analysis software that 
performs collision detection cannot check whether all the 
constraints are truly independent of each other, and only 
during simulation can it be determined that some constraints 
are redundant. Such degeneracy can cause real problems for the 
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simulations, especially in the case of collision detection 
which checks for proximity of pairs of objects, whereas the 
constraint degeneracy only appears at the system level 
including all the rigid bodies in the system. 

Fifthly, known systems do not cope well with stiffness, 
i.e. rigid spring-like systems and compliant elements. The 
only tractable solutions ignore contact and friction 
altogether, which makes them unsuitable for analysis of 
arbitrary physical systems. 

Accordingly, there is a need for an image display system 
that ameliorates or alleviates some or all of these 
difficulties. 

The known models require the solution of linear 
complementarity problems, a particular type of constrained 
equation. In general, a linear complementarity problem can be 
put in the form: 
Mz + q = w 

Zi > 0 Vi e {l,2~ai} (2 ) 
*i > 0 Vi e {1,2..ji} (3) 
XiWi =0 Vi e (l,2^n} (4) 
where M is an n by n matrix and z and w are real n- 
dimensional vectors. The problem requires finding the solution 
of equation (1), i.e. the values of z and w, subject to the 
constraints (2) - (4) . 
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This is fundamentally a combinatorial problems, and 
solutions generally search through two index sets, where each 
index is in one of the sets. The first set a is a set of 
active variables for which Wi = 0 and the second set p is a set 
of free variables for which Zi = 0. The problem is then 
partitioned as 











la' 




' 0 " 
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+ 









(5) 



where a and P specify indices in the first and second sets 
respectively. 

This is equivalent to the linear algebra problem 



which must be solved for z while w is calculated by 
substitution. M aa is known as the principal submatrix. 

Various implementations are known. They differ in how the 
two sets are revised. They use a computed complementarity 
point which is a vector s such that 



The methods go through a sequence of sets until a solution is 
found, i.e. until Si > 0 Vi e {l,2..si}. 

The Murty principal pivoting algorithm is known from Murty 
and Yu: "Linear Complementarily, Linear and Non Linear 
programming" available at 




(6) 




(7) 
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www . personal . engin . umich . edu/ -mur ty/book/LCPbook/ index . html , 

and also in an earlier edition published by Helderman-Verlag, 
Heidelberg (1988), the content of which is incorporated into 
this specification by reference. 

The indices are assigned to a set, and i is set to zero. 
Then, the principal submatrix M aa is formed and solved for z 
using equation (6) . 

Then s a> = z a + w p is calculated, where 

Wfi = Mp a Z a ( 8 ) 

If s > 0 then the algorithm has found a solution. Otherwise, 
the smallest index j for which the corresponding element of s 
is found. If this index is in the first set, the index is 
moved to the second, otherwise the index is in the second set 
in which case it is moved to the first set. The loop parameter 
i is incremented and the loop restarted until a solution is 
found. 

The method is illustrated in the flow chart of Fig. l. 

The method is stateless and can be started from any 
initial guess for the division of the indices into first and 
second sets. The matrix will work on any P matrix and in 
particular on any positive definite matrix. 

The method can fail where the matrix is positive semi- 
definite. Such matrices arise in real physical systems, and 
can be made positive definite by adding a small amount to each 
element along the diagonal. Kostreva in "Generalisation of 
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Murty's direct algorithm to linear and convex quadratic 
programming", Journal of optimisation theory and applications, 
Vol. 62 pp 63-76 (1989) demonstrated how to solve such positive 
semi -definite problems. Basically, the problem is solved for 
5 an initial value of s, e is then reduced until the solutions 
converge; if the solutions diverge the problem is unfeasible. 

SUMMARY OF THE INVENTION 

According to the invention, there is provided a method, a 
10 computer program recorded on a data carrier (e.g. a magnetic or 
optical disc, a solid-state memory device such as a PROM, an 
EPROM or an EE PROM, a cartridge for a gaming console or another 
device), for controlling a computer (e.g. a general purpose 
micro, mini or mainframe computer, a gaming console or another 
15 device) having a display screen, a memory and a processing 
unit, the computer program being operable to control the 
computer to carry out the method, and a computer programmed to 
carry out the method. 

The method according to the invention may include the 
20 steps of: 

storing in a memory position and velocity parameters 
defining an initial state of a model system having a plurality 
of bodies, 
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storing in the memory parameters defining at least one 
constraint function constraining the motion of the bodies in 
the model system, and 

calculating in the processor the position and velocity 
parameters defining the state of the system after a 
predetermined time step based on rigid body dynamics, including 

carrying out a semi-implicit integration step subject to 
the constraints, to determine the velocity after the step, 
including 

determining the constraint forces that act to keep the 
system in compliance with the constraints by ensuring that the 
first derivative of the constraint function is zero. 

In known approaches, the second derivative was held to be 
zero. However, the method according to the invention provides 
much faster calculation. 

The method may cause the computer to carry out the further 
step of displaying an image of the objects at their calculated 
positions on the computer display screen, so that the display 
shows the objects on the screen using physical laws to simulate 
their motion. 

The means for determining the constraint forces may 
include solving the linear complementarity problem using the 
Murty algorithm. 

The calculating step may include carrying out the implicit 
integration by 
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calculating the velocity parameters after the time 
step from the external forces, the constraint forces and 
the position and velocity parameters before the time step, 
and 

calculating the position parameters after the time 
step from the external forces and constraint forces, the 
calculated velocity parameters after the time step and the 
position parameters before the time step; and 
In prior art image display methods implementing rigid body 
dynamics the accelerations have been taken as parameters. In 
the method according to the invention, the parameters 
calculated are position and velocity. 

The means for solving the linear complementarity problem 
may include solving the boxed LCP problem by the modified 
Murty's method. 

In order to implement maximum bounds on the constraint 
forces the calculation may include the step of testing whether 
the constraint forces have a magnitude greater than a 
predetermined value and if so setting them to be that 
predetermined value. This has not previously been done but 
leads to a more efficient solution. 

Preferably, the model includes a model of friction. 
Static friction requires that the tangential force f t of 
magnitude less than or equal to the static friction coefficient 
Us times the normal force f n . The force f t due to dynamic 
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friction has a magnitude equal to the dynamic friction 
coefficient fi 3 times the normal force, and a direction given by 
ft v t < 0. 

The dependence of the frictional force on the normal force 
can be replaced by a novel approximation in which the friction, 
dynamic or static, is not dependent on the normal force. This 
force then corresponds to a simple bounded multiplier, i.e. a 
force that can have up to a predetermined value. Thus the 
force exactly fits the model used in any event in which the 
constraint forces Lagrange multipliers have maximum values; 
friction in the model is equivalent to another bounded 
constraint force. Thus making this approximation substantially 
simplifies the inclusion of friction in the model. 

Accordingly, the method may include a model of friction in 
which the frictional force between a pair of objects is 
independent of the normal force between the objects. 

The frictional force between each pair of objects may be 
modelled as a bounded constraint force in which the constraint 
force acts in the plane of contact between the pair of objects 
to prevent sliding of one of the pair of objects over the other 
of the pair, wherein the constraint force is bounded to be not 
greater than a predetermined constant value to allow sliding of 
the objects over one another and thus include dynamic friction 
in the simulation. 
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In order to implement bounded constraint forces, the 
method may include the step of testing whether the constraint 
forces have a magnitude greater than a predetermined value and 
if so setting them to be that predetermined value. 



to determine how exactly to satisfy the constraints. 

The friction model taken leads to a symmetric positive 

definite linear complementarity problem, in which the only 

friction conditions are simple inequalities. This allows the 
10 use of the much more efficient Murty algorithm that the less 

useful Lemke algorithm. 

From further aspects, this invention provides a computer 

program that is a computer game program, which game program may 

be recorded within a cartridge for a computer game machine; and 
15 a computer game programmed to generate a display by means of a 

computer program according to any preceding claim. 



5 



The method may include a relaxation parameter y introduced 



BRIEF DESCRIPTION OF THE DRAWINGS 



20 



Specific embodiments of the invention will now be 



described, purely by way of example, with reference to the 



accompanying drawings in which 



Figure 1 shows a flow diagram of a prior art 



implementation of the Murty algorithm, 



25 



Figure 2 shows a computer running the present invention, 
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Figure 3 shows a flow diagram of the implementation of the 
present invention, 

Figure 4 shows the friction pyramid, 

Figure 5 shows the box- friction model used to approximate 
to the friction pyramid, 

Figure 6 is a flow chart of the method including friction, 
and Figure 7 is a flow chart of the program according to the 
invention. 



DETAILED DESCRIPTION 



The implementation of the invention that will be described 
includes a computer system 1 having a display 3, a memory 5 and 
a processor. The computer system has software 9 loaded into 
the computer memory to allow the computer system to display a 
simulation of the real physical world on the display 3. The 
display 3 may be a conventional computer display screen, for 
example an Liquid Crystal Display (LCD) screen or a Cathode Ray 
Tube (CRT) screen, or the display 3 may be a less conventional 
display type such as virtual reality goggles or multiple 
screens of a coin operated video game, of the type installed in 
public places. 

A physical system which has n rigid bodies is modelled. 
The 1 th body has a mass m if and a position vector p which has 
seven coordinates, three to define the position of the rigid 
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body and four being the Euler parameters of the body defining 
its orientation. Each body also has velocity vector v which 
has six coordinates, three being the three components of linear 
velocity and three being the three components of angular 
5 velocity, each relative to the inertial frame. Further details 
about the coordinates and the matrices used to convert between 
coordinates in the local reference frame and those in an 
inertial frame are given in sections 1 to 1.4 of Appendix 1. 
The key point to note is that equations 1.3 6 and 1.3 7 of 
10 Appendix 1 define Newton's laws for a dynamical system in a 
form similar to equation (1), i.e. 

M = fe+fc+fr O) 

where M is the block diagonal matrix defined in equation 1.37 
of appendix 1 . 

15 A rigid body dynamics problem then becomes equivalent to 

solving equation (7) subject to the dynamic constraints. This 
is carried out by numerical integration. The difficult part is 
calculating the constraint force. 

The initial state of the system is set up in the computer 

20 memory, and includes the above parameters. 

Constraints governing the rigid bodies may also be set up. 
The software 9 may include definitions of such constraints. An 
example of a constraint is a hinge between two rigid body 
elements so that the elements cannot move independently. The 

25 system contains constraint initialisation routines for setting 
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up such constraints. A separate routine is provided for each 
type of constraint; the routine is called, defining the one, 
two or three constrained elements and other information 
required to specify the constraint. For example, if there are 
two constrained elements in the form of two rigid bodies are 
joined by a ball and socket joint, the information required is 
the identity of the constrained elements and the position of 
the ball and socket joint. The constraint information is 
stored in the form of the Jacobian of constraint. 

A simple example of a constraint would be a rigid floor at 
height zero in the model. The constraint function <t>(p x ,p y ,pj 
is then chosen to be O(p) = p z , the conventional constraint 
0(p) > 0 then being a definition of the constraint. 

The solution method used requires a Jacobian J of the 
constraint function 0(p) - this is related to the more 
conventional Jacobian J p of the function 0(p) with respect to 
position by J = j p Q where Q is defined at equation 1.40 of 
Appendix 1. 

The method used does not require storing the second 
derivative of the constraint function. 

After the initial conditions and constraints are set up, 
the routine may be called to simply step forward in time by a 
predetermined time period. Indeed, the use of this simple 
program structure in which a routine is called with a 
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matrix and outputs a like matrix of the results one time step 
forward is a significant advantage over prior approaches in 
which the movement forward in time has not been encapsulated in 
this way. 

5 The way in which the system moves one step forward is 

based on simple Euler integration, i.e. calculating the final 
positions and velocities from the initial positions and 
velocities and the applied forces. Of course, some of the 
forces are the constraint forces that ensure that the system 
10 remains in accordance with the constraints; the way these are 
calculated will be described below. 

Euler integration can be explicit, in which the 
integration is based on the values at the start of the step, or 
implicit in which the values at the end of the step are used. 
15 In the method of the invention, a semi-implicit approach is 

used in which the positions after the step are calculated using 
the positions at the start of the step and the velocities at 
the end of the step and the velocities are calculated 
explicitly. Put mathematically, the position p and velocity v 
20 at the (i+l) th step after a time h are given by 

Pi+i = Pi + h v i+1 (10) 
Vi+! = Vi + h. MT l .f (11) 
where M is the block diagonal matrix defined in Appendix 1 
and f is the sum of the forces on the system, including the 
25 constraint forces to maintain the constraint. Note that the 
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equations for the position variables are implicit and the 
equation for velocity explicit. Thus, the above equations need 
to be solved subject to the constraint equations. 

Of course, in order to carry out the above calculation it 
is necessary to calculate f . The force f is made up of the 
external force plus the effects of the external torques plus 
the constraint force that keeps the system in accordance with 
the constraints. Thus, the constraint forces on the system 
must be calculated. Appendix 1 at 1.51 demonstrates how to do 
this for an explicit Euler scheme to calculate subject to the 
constraint 0(p) = 0. In the conventional scheme, the 
constraints are calculated by setting the second derivative of 
Q> to zero. 

In the method according to the embodiment, however, this 

is not done and the first derivative — is set to zero 

dp 

The detail is set out in sections 1.5 to 1.6 of Appendix 
1. The approach of using the velocity constraints rather than 
the second derivative of the constraint function has both 
advantages and disadvantage. The key disadvantage is that 
although this approach guarantees that the velocity constraints 
are satisfied it does not guarantee that the position 
constraints are. 

The constraint equation may be given by 

<Po + J P (p' - p) =0 ( 12 ) 

SUBSTITUTE SHEET (RULE 26) 



WO 01/67310 



17 



PCT/GB01/01020 



where J p is the Jacobian of the constraint forces based on 

the position, i.e. J =— . This is related to the J actually 

dp 

calculated by J = J P Q. 

Rather than satisfy this exactly, the parameter y is 
introduced by amending the right hand side of equation (5) so 
that it reads 

<f>o + J P (p* - P) = (l-r)fa (13) . 

When y - 1 equation (6) becomes equivalent to equation (5) . 
A value of 0.2 has been found suitable. 

A solution for the forces is given at 1.5.5. of Appendix 

1. 

The result is : 

(jM-*J T y = ^P--^-JM-\f e +f r ) (14) 
h h 

This is an equation in the form Ax + b = w and it can be 
solved by the algorithm presented below to find the vector X. 
The constraint force f c is then given by 

f c = J r A. (15) 

The constraint forces may then be fed into the integration 
step to compute the position of each object after the timestep. 

It should be noted that the method uses bounded 
constraints. In other words, each element of the force is not 
allowed to become arbitrarily large as in previous methods but 
is bounded. This is simply implemented by testing to see if 
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the elements of the force are larger than the bound and if so 
reducing the element to the bound. 

The problem posed is not directly amenable to simple 
solution by the Murty algorithm. Rather, it has the slightly 
different structure of a boxed linear complementarity problem, 
as follows: 

Az + q = w+ - ( 16 ) 
zi - li > 0 
w+i > 0 

(Zi - li) w+i = 0 
Ui - Zi > 0 
w_i > 0 

(Ui -Zi) w_i = 0 

The w+ and w. terms come from the integration step; they 
correspond to forces/accelerations from upper and lower 
boundaries respectively, the positions of the boundaries being 
given by u and 1. The z term corresponds to the differential 
of the velocity. 

The result gives the constraint force which can be plugged 
into the integration. 

The above formalism is equivalent to a mixed 
complementarity problem defined as 



" A 


-If 


z 








"0" 




I 


0 0 


w 
+ 


+ 


-i 




n 


(17) 


-I 


0 0_ 






u 




V 





SUBSTITUTE SHEET (RULE 26) 



WO 01/67310 



PCT/GB01/01020 



19 



This can be solved by partitioning the second set defined 
above into two sets, y and i, so that Zj = lj for j e y and Zj - 
Uj for jet. Afterwards, the Murty algorithm is followed with 
a different definition of the complementarity point, namely, 



The least index rule is applied to the complementarity 
point as follows: 

j = min arg (Sj <0) 
10 If j e a and Zj < lj then remove j from a and put it in y 
If j e a and Zj > Uj then remove j from a and put it in i 
If j e y add j to a and remove it from y 
If j £ i add j to a and remove it from i 

The loop is then repeated until there is no element Sj < 0. 
15 Figure 3 sets out a flow chart of the Murty algorithm that 

is used to model the constraints. It is based on that of 
Figure 1, with amendments to cope with the bounded parameters 
which are may be used to model friction. 



20 solution, which has been developed independently by the 
inventors . 

The above solution may be refined by replacing the zeroes 
with a tolerance parameter - e. This is similar to the Kostreva 



5 




min(z y -l j9 Uj-Zj) Vjea 



(18) 



This solution will be referred to as the Boxed LCP 
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method, except here the parameter is not reduced but simply 
kept at a constant value say 10" 3 . 

The parameters y and e may be chosen to model a compliant 
coupling between two of the rigid bodies. In this way springs 
and compliant bodies may be included in the model without any 
complexity. 

Appendix 4 sets out how the parameters e and y already 
present in the model can be used to model stiff springs. Such 
parameters are very useful for modelling car suspensions, and 
the like. Thus, the use of the model provides the unexpected 
benefit of being usable not merely to ensure fitting of the 
constraints but can also be used to model stiff springs without 
any additional parameters or programming. 

A key advantage provided by the approach selected is that 
it allows the inclusion of friction. 

The friction constraint can thus be considered to be given 
by a cone which; if the contact force between two bodies is 
given by a force vector the values of the dynamic friction when 
the bodies slide is given by a cone in the three dimensional 
space of the force (Figure 4). In the invention this cone is 
approximated by a four sided box, in other words friction is 
approximated by a model in which the transverse frictional 
force is not dependent on the normal force (Figure 5) . 
The method thus works as follows: 
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Step 1: For each contact point, apply a non-penetration 
constraint and a static friction constraint. 
Step 2: Estimate a normal force Xi for each point 
Step 3: At each point of contact, limit the Lagrange 
multipliers that enforce zero relative velocity to have a 
maximum value proportional to the normal force : i.e. 

-\xXi < Pij < \iXi 
Step 4: solve with the boxed Murty algorithm 
Step 5: refine estimate of normal forces and repeat if 
required . 

A flow chart of this procedure is presented in Figure 6. 

Surprisingly, such a crude model still gives good 
realistic results. The real advantage is that the model does 
not involve a combination of the Lagrange multipliers 
(constraint forces) as a constraint - the constraints are of the 
simple form f c < a constant, whereas for more realistic model 
the upper bound on f t would depend on the normal force. This 
allows the use of a simpler algorithm as set out above. 
Indeed, the bounded constraint force has exactly the same 
bounded force as used for all of the constraints on the 
objects; accordingly adding friction does not add significantly 
to the difficulty of solving the problem. 

Thus, the described embodiment allows much faster 
processing of the simulation. 
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A list of the routines used in the program implementing 
the embodiment, including the many routines to set up 
individual types of constraint, is provided in Appendix 2. 

The program implementing the above is schematically shown 
in Figure 7. 

Firstly, a body data array containing the information 
about each rigid object, i.e. its position, orientation and 
mass is initialised. A constraint data array is likewise 
initialised containing parameters defining the constraints. 

In the next step, the constraint data array is 
interrogated to create a list of Jacobians. 

Then, a matrix A is calculated given by A = JhT 1 ^ where M 
is the mass matrix as defined in Appendix 1 and J is the 
Jacobian. This step is done so that A is an upper triangle 
matrix (and symmetric) . 

The upper triangle elements of A are then copied to the 
lower, and the diagonal modified. Rotational force may be 
added to the bodies at this stage. The A matrix is then 
complete. 

Next, A is factorised to find A' 1 by Cholesky deposition. 
An intermediate result rhs is then calculated as follows: 
Calculation of rhs, for each body: 
tmp =0 
tmp = MT 1 .f e 
tmp = tmp + v/h 
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rhs = c/h - y/h 2 - J.tmp 

Then, rhs, A, A' 1 and the lower and upper constraint 
vectors 1 and u are sent to a routine which calculates \, the 
Lagrange multipliers, by solving equation (16) by these 
5 parameters by the boxed LCD method as described above. 

Next, the resultant forces are calculated from X and J, 
and the results passed to the semi -implicit integrator. 

This outputs the velocity, force, position and 
orientation, in the form of the transformation matrix and 
10 orientation quaternion of each body. 

Finally, a screen image is calculated displaying the 
objects in their new locations and this is displayed on the 
video display unit. 

The method described works much faster than previously 
15 known methods. Accordingly, it becomes possible to more 

accurately simulate real time scenes, and provide an improved 
simulation that more rapidly and accurately simulates the real 
physical world. 
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Appendix 1 



1 Rigid Body Dynamics 

1.1 Definitions 

Acronyms 

• COM: Center of mass. 

• IF: Inertial frame of reference for the whole system. 
Notation 

• a (any 3 x I vector) is relative to the IF. 

• d is relative to the coordinate system of some rigid body, a and d are related by a uni- 
tary (rotational) transformation. 

• /, is the identity matrix of size / x / . 

• 0, is the zero matrix of size I x i . 

• If a and b are 3 x I vectors then 

a x b = ah (U) 

where 



a ~ 



0 a>> 
a 2 0 -a x 
-a*, a, 0 



X cu = O (1.2) 



^F(p) = V p F(p) (1.3) 

1.2 Parameters of the mechanical system 

• There are n rigid bodies ("links") numbered 1 ...» . The coordinate system for each body 
has its origin at its center of mass. 

• Link i has mass hi, and 3 x 3 body-relative inertia tensor H t . 

• There are /// constraint equations. 
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• p (In x l ) is the system position vector, which has the structure 

p= [>i ft #y] T < l4 > 

where 

/>/ = [*i * * ll if 4 iff < L5 > 

where (.v,y, z) is the position of the center of mass and (q l 9 q 2 9 e/\q 4 ) is the orientation 
quaternion (Euler parameters) These arc both relative to the IF. 

• v (6n x 1 ) is the system velocity vector, which has the structure 

v = [v, v, ... ij r (1-6) 

where 



where (.v, y, i) is the velocity of the center of mass and (co 1 , co 2 , co 3 ) is the angular velocity 
(both relative to the IF). 



R = 



q x q l + q 2 q 2 -q*q z -q*q* 2q 2 q*-2q ] q 4 

2<?V + V<? 4 
-2q>ql + 2q 2 q* 



q x q ] -q 2 q 2 + q*q* -q 4 q 4 - 2q ] q 2 + 2q*q 4 



(1.8) 



Rj is the 3 x 3 unitary rotation matrix corresponding to the Euler parameters for body / 

2q l q* + 2q 2 q* 
-2q*q 2 + 2q z i 
2q x q 2 + 2q z q A _ q l q [ - q 2 q 2 - <?V + q 4 q\ 

Force vectors (such as f e , the external force) have the structure 

f~ [fxh •••/«]' < ,9) 

where 

= \fffIffTfTJlf (l.«0) 

where [f\f>\f ; ) is the force applied to center of mass and (F, 7 V , r*) is the applied torque 
(both relative to the IF). 

1.3 Equations of motion for one rigid body 



1.3.1 Coordinate transformation 
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R is the unitary transformation matrix for link i (3 * 3 ). If a is a point in the IF and d is 
point in the link frame then a = Ra\ Also note that 



/M = R T 

The ith column vector of R is w f> so that 



R = 



The m. vectors are axis unit vectors for the body. 
1.3.2 Rotating frame 

If a is an IF point in the body relative to the center of mass then 



a = co x a 



Thus 



R = [w, ii 2 i/ 3 ] 
= [co x w, co x u 2 CO x i/ 3 j 

= [(jOWj Cbi£ 2 6w 3 J 



= <bR 



(1-11) 



(1.12) 



(1.13) 

(1.14) 
(1.15) 

(1.16) 
(1.17) 



Note that 6 = -co 7 ' and coco =0. 
1.3.3 Angular momentum 

To start with we will consider just the rotational part of motion, and ignore the linear 
(which is a lot easier to deal with). The definition of angular 
is 



part 



momentum L for a rigid body 



1 1> x Ufa 



(1.18) 



where q, are the positions of all particles in the rigid body (w.r.t. the IF) and M, are their 
masses. Now, 



L = Z£ ( «< x M ^ 



(1.19) 
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= £ </, x + £<7,- * M,?,. ( 1 .20) 

= 2>*«, (L2,) 
where g. is the force on particle /. Now, 

where » l7 is the force between </■ and <y ; , and G, is the external force on q- . It can be shown 
that if g.j - -fy (as is the case according to Newton) then the total contribution to L from 
the g.. is zero, so 

L = ^.xC, (1.22) 

Where the sum over / only includes those particle positions at which an external force is act- 
ing. Introducing the effect of torque is easy - all "pure" torques applied to the rigid body 
effectively works through the center of mass, so 

L = £*,xG, + £t, (1.23) 
i 

The quantity L is effectively the external rotational force applied to the body, which is usu- 
ally a combination of joint constraint forces and forces external to the rigid body system. 

1.3.4 The link between co and L 

It can be shown that 

U = //co' (1.24) 
where L = RL\ co = flee' , and H is the 3 x 3 inertia tensor for this body. Thus 

R T L = HR T <» (1.25) 

L = RHR r co (1.26) 

from the chain rule 

L = ~{RHR T )u + RHR r (b (1.27) 
at 

= (RHR r o) + RHR T ai + RHR r (b) (1.28) 
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R T a = (&R) T a 
= R T d) T <a 
= 0 



so 



RHR T <b = L- RHR T w 

= L-(b(RHR r )(o 
= L-iox ((RHR T )u) 

or, schematically, 

(inertia tensor in IF) x (angular acceleration) = (external force) + (coriolis force) 
which is simply a statement of Newton's law. 



(1.29) 
(1.30) 
(1.31) 

(1.32) 
(1.33) 
(1.34) 

(1.35) 



1.4 Equations of motion for a rigid body system 

For n rigid bodies we simple combine the equations of motion into one matrix equation: 

M ^=fe+fc+fr (1.36) 

where M is a block diagonal matrix (with 3x3 blocks): 



M = 



m,/ 3 



R,H,RJ 



m-,1. 



R 2 H,RT 



m„l 3 



(1.37) 



and f t is the external force (applied from "outside" the rigid body system), f c is an undeter- 
mined constraint force, and f r is the rotational force: 



-co, X f)co,) 



(1.38) 



The state variables of the system are p and »-. To integrate the system, the state derivatives 
must be known. The above equation allows us to determine i, assuming that f f is known). 



SUBSTITUTE SHEET (RULE 26) 



WO 01/67310 



PCT/GB01/01020 



29 

P is determined by applying a transformation that allows the angular velocity to be con- 
verted into Euler parameter derivatives: 

P = Qv (1.39) 



Q = 



where 



D { = 0.5 



-ll -it -1? 
+<lt 

-it 

+<i? -<ir +q! 



Note that £>/*£>, = / 3 /4, 



1.5 Integration with implicit constraint projection 



1.5.1 Explicit Eulcr 



(1.40) 



(1.41) 



The explicit Euler integration step is 

new/? = /?* = p + hQv (1.42) 
new v = v* = v + hM-*(f e +f c +f r ) (1.43) 

As the system evolves we want to compute f c such that the following constraint is satisfied: 

<(>(/?) = 0 (1.44) 
Note that § (j?) has size m x I . Make a first order approximation to this: 



(1.45) 

n.46) 
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where p 0 is the current position, and J p is the m x 7/» Jacobian of the constraint function 
with respect to position (evaluated at p Q ). Find the first derivative of 4 : 



op dt 

= J P Q*' 

= Jv 



(1.47) 

(1.48) 
(1.49) 
(1.50) 



Where J is the Jacobian of the constraint function with respect to velocity (J = J p Q). Find 
the second derivative of <(> : 



dp 



$ (p) = — • + 21 . 

2 

3(f) • • 
= Jv - c 

7 is a tensor quantity because <|> is a vector. 



(1.51) 
(1.52) 

(1.53) 
(1.54) 



Note that during simulation, J is the matrix that we actually calculate, not 
J p . The constraint force f c is computed as 

fr = ' (1.55) 

where /. is a vector of Lagrange multipliers. This ensures that the minimal amount of work 
is done to enforce the constraints. Compute f c such that $ (p) = 0 : 

Jv = c (1.56) 
JM-Hf e + jTx+f r ) = c (L57) 

JM-IJT\ = c-JM-*(f e+ f r ) (|.58) 

which gives X , from which f e and v can be computed. Note that we can get the same result 
from solving 



(1.59) 



M J T 




V 




f e +fr 


J 0 




X 




c 
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1.5.2 Implicit Eulcr 

The implicit Euler integration step is 

p* = p + hQ(p*)v* (1.60) 
v* = v + hM-*(p*)(J,Q>\v*)+f c (p\v*)+f r Q>\v*» ( ,6I > 

where the dependence of the right hand sides on the new state is made obvious. Performing 
this fully implicit update is rather difficult, even when using low order Rosenbrock methods, 
for reasons explained later. 

1.5.3 Velocity projection method 

Instead of trying to satisfy the acceleration constraint (<j> = 0 ) lets try to satisfy the velocity 
constraint (<{> = 0). From equation 1.50: 

<fr(p) = Jv = 0 (1.62) 

Substitute the Euler velocity update (equation 1.43) into this, and solve for X : 

J(v + hM-t(f e +f c +f r )) = 0 (1.63) 

(JM'iJ T )X = c-~-yM- l (/,+/ r ) (1.64) 
n 

which is similar to equation 1.58 except that the term -Jv/h has replaced c. f c and the new 
state are computed as before. This equation ensures that the velocity constraints are met at 
the new state, which reduces constraint violation compared to equation 1.58. It also means 
we do not have to have a separate impulse application step- collision constraints for exam- 
ple will automatically result in a zero penetration velocity. 

A further advantage is that the value c does not have to be computed for each constraint. 
Although this thing is relatively inexpensive to compute, it is annoying to derive. 

1.5.4 Position projection as a separate step 

Extra work must be done in the velocity projection method to ensure that the constraints 
don't come apart, because although the velocity constraints are guaranteed to be satisfied at 
the new state, the position constraints may not be. One option is Baumgarte stabilization. 
Another is direct position projection. We start with 

♦n + -/.(/i-Pn) = 0 (1.65) 
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where $ 0 and J p are re-evaluated at the new position. J p is usually not square so there are 
many possible p that satisfy this equation. We want to find the solution which minimizes 
\p-Po\ ■ This is done by setting 

P-Po = jT P ? > (1.66) 

so that 

4o + v;s = o (1 . 67) 

S = -(Vr)~'*o (1-68) 
P*-Po-(JpJj)-% (1.69) 
This corresponds to one iteration of a Newton method to find the root of <(>(/;) = 0 . Multiple 
iterations can be performed to get perfect projection, but this will usually not be necessary. 

1.5.5 Position projection method 

We can take this process one step further by trying to satisfy the position constraint 
(<K/>) = 0 ) at the new timestep. To do this we must first express the Euler position update in 
terms of the new velocity rather than the current one, otherwise the new position will be 
independent of /. : 

»* = v + hM-*(f e + jT\ +fr) (]70) 
p* = p + hQv* ( , 71) 

so that 

p* = P + hQ{\> + hM-i(f e + Ji\+f r )) (172) 

The constraint equation we want to satisfy is (to first order) 

h + J p (P*-p) = 0 d.73) 

But actually to gain more flexibility we will introduce a parameter y which controls how 
much we want to satisfy this constraint, so: 

h + J p lj>*-p) = (l-YWo (1.74) 

so if y = o we are assumed to already have the correct position and no position projection 
will be done (y = | ) will give us normal position projection). So 
to + J p hQ(v + hM-% + jT\ +/,)) = ( , _ y)+o 
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h\j p QM-*jT)\ = -^ Q + hJ p Q(v + hM-*(f e +f r )) (1.76) 
(JM-*jr)X = J^J^-ju-x^+ft (1 . 77) 

which is similar to equation 1.64 except for the addition of the term -y<t> 0 //i 2 . This equation 
ensures that the position constraints are met at the new state, which reduces constraint viola- 
tion compared to equation 1.64. It also means 
thai a separate position projection step is not required (position projection here is consider- 
ably less expensive). 

1.5.6 Summary 

For a standard Euler update, satisfy acceleration constraints by 

(JM-*jT)k = c-JM~*(f e +f,) (1.78) 
For a standard Euler update, satisfy velocity constraints by 

{JM-ijT)\ = - J -l-JM-*(f e +f r ) (1.79) 

For a modified Euler update, satisfy position constraints by 

(JM-iJT)\ = J^Jj-JM-^+fj (Lg0) 



2.0 Contact with friction 

The Coulomb friction model relates the normal and tangential force at each contact point 
between two bodies. The distinction is made between static and dynamic friction. With 
static friction there is no relative movement (sliding) between points that are in contact, and 
the following equation is satisfied: 

M*vK at) 

where K a is the (scalar) normal force, I, is the 2 x 1 tangential force, and \i is the static 
friction coefficient. With dynamic friction the two surfaces are in relative motion and a dif- 
ferent equation applies: 

K = (2.2) 
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where v is the relative surface velocity vector (2x1) and u rf is the dynamic friction coeffi- 
cient. Equation 2.1 defines a friction cone, the boundary between static and dynamic fric- 
tion. 

With acceleration based friction models such as those used by Baraff the distinction 
between static and sliding friction is made explicitly, and care must be taken to ensure that 
switching between the two modes happens correctly. Baraff s acceleration based friction 
model generates an LCP problem that can sometimes be inconsistent, and even when it is 
consistent it is not guaranteed to be solvable by his Cottle-Dantzig based LCP method. 

2.1 Velocity based model 

The velocity based formulation of Anitescu and Potra resolves some of these problems. 
First, to allow a linear complementarity solution the friction cone is approximated by a fric- 
tion pyramid (figure 2.1). The pyramid has s sides which are given by the 2 x 1 vectors 
c v ..c s . The conditions corresponding to equation 2.1 are: 

V.: cf(^-^a„c,)<0 ( 2.3) 

c /VmV/"c,.£0 (2.4) 
-cTX l + i i\ n >0 (2 .5) 

Each side of the friction pyramid has a scalar velocity residual a,.. When the contact is slid- 
ing in a direction corresponding to side /, a,, is positive. When the contact is within the fric- 
tion pyramid, all the a ( . are 0. The equation of motion is 

S 




C 3 ' 




v C 2 



Figure 2.1: The approximated friction cone. 



SUBSTITUTE SHEET (RULE 26) 



WO 01/67310 



PCT/GB01/01020 



35 



where 7, extracts from the system velocity vector the 2 x I velocity vector in the contact 
plane. The full system for m contact points is (as a tableau): 





X,, ... X m 


\\ — Km 


a,, ... a,, 


a „,i ••• a „ ls 


M -J] 
J j 


J t\ J tm 


_/T _/T 






^ I Hi 






r, ... c s 


c, ... c s 


h\ 












-cj 












V- 







fe+fr 

= 0 



= 0 
= 0 



>0 
>0 



>0 
>0 



>0 
>0 



(2.6) 



where J } is the joint Jacobian, J ti is the tangential velocity Jacobian for contact /' and J nl is 
the normal velocity Jacobian for contact i. In matrix notation, 



H 



= q 



(2.7) 



Note that the friction model is anisotropic, and that 
there is no separate value of u. for static and dynamic friction. 

2.1.1 How to implement this 

Directly implementing an LCP solver for the above model is wasteful, as the matrix that 
needs to be factorized is larger than necessary (there are 3 + s rows for every contact point). 
A method that uses only 3 rows per contact point will be described below. Consider the 
statement of standard LCP: 

Ax = b + w (2.8) 
-r > 0 (2.9) 
h- > 0 (2.10) 

X,W; = 0 (2.11) 
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The matrix A has size n x n . The Murty principal pivoting algorithm to solve this problem 
goes like this 

• Set a = {!,: „} . Here a is the set of indexes of "clamped" variables. 

• Loop 

-Solve for x:x s = A&b B ,x u = o. 

- Find the residual w : w = Ax-h. 

- If any elements of x are less than -e , find the first one and add its index to a . 
Restart the loop. 

- If any elements of «• are less than -e . find the first one and remove its index from a . 
Restart the loop. 

- If we get to this point, we have a valid solution (x, w) . 

This procedure is guaranteed to terminate if A is a P matrix. The value e is a small toler- 
ance, typically 10 -6 . A variant of this procedure that is more computationally efficient in 
practical situations finds the value of x at each iteration by projecting from the totally 
undamped solution A-'b. At each iteration we ask for the equation Qx = 0 to be true, 
where Q is a matrix with a row for every index in a, with a single 1 in the position corre- 
sponding to the index (this is equivalent to asking for all the x a to be zero). 

The projection is done like this: 

define \V a such that w = U'^ (2 12) 

x = A-U)+A-^W a \v u 
QA-*b + QA-iW aWa = 0 (as Qx = 0) 



(2.13) 
(2.14) 



% = -(QA- l W a )-iQA-ib (2.15) 
% = HQA-HV^Qx, ( 2.i 6) 
w a = 0 (2.17) 

where 

.v 0 = A~ l b ( 2. 18) 
The value of w a is substituted back to get x. 

Now, in our case some of the variables in x correspond to normal forces (X„ ) and some vari- 
ables correspond to tangential forces (X lx , \,,). If the normal force conditions are violated 
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(X n < 0 ) then the normal force can be projected back to zero as above. If the tangential force 
conditions are violated, the tangential forces must be projected back to the planes that make 
up the sides of the friction cone. This can be done by simply altering Q so that the appropri- 
ate plane equations appear on the rows of Q corresponding to the projected tangential vari- 
ables. 

This is easiest to achieve when we have a four sided friction pyramid, because then X IX and 
/ Vv are effectively decoupled from each other. Also this means thai a tangential variable will 
be projected to at most one plane at a time (if we had a many-sided friction volume then 
many planes could be involved in a projection and the situation becomes more compli- 
cated). 

Here is the modified Murty algorithm: 

• Make the matrices Q n y Q h and Qj of size itxn such that Q n x - 0 is the condition for all 
normal forces to be zero, Q h x = 0 is the condition for all tangential forces to be 
clamped at the "high" plane, and Q l x = 0 is the condition for all tangential forces to be 
clamped at the "low" plane. 

• Set a„ = a h = a, = { } . Here a„ is the set of clamped normal forces, a { and a,, are the 
sets of clamped-low and clamped-high tangential indexes. 

• Loop 

- Set a = a M n a, n a h 

- Set Q : 



Q = 



Q 1 



(2.19) 



• Solve for x : 



w a = -(GA-'Hy-'Qxo 
vv fi = 0 
X = a- 0 + /H W>' a 



(2.20) 
(2.21) 
(2.22) 



- If any normal elements of x are less than -e, find the first one and add its index to 
a n . Restart the loop. 

- If any elements of Q l x are less than -e , find the first one and add its index to a, . 
Restart the loop. 
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- If any elements of Q\ are less than -e, find the first one and add its index to a„ . 
Restart the loop. 

- If any normal elements of w are less than - e , find the first one and remove its index 
from ct„ . Restart the loop. 

- If any ciamped-low tangential elements of w are less than -e . find the first one and 
remove its index from a,. Restart the loop. 

- If any clamped-high tangential elements of w are greater than c , find the first one 
and remove its index from a /( . Restart the loop. 

- If we get to this point, we have a valid solution (x, w) . 

This algorithm simulates what happens when the full problem in the previous section's tab- 
leau is solved with the standard Murty algorithm. This problem is guaranteed to have at 
least one solution, but unfortunately this algorithm is not guaranteed to find it. As u gets 
larger the algorithm will start to cycle though the same index sets. In practice this rarely 
happens with physically realistic values of u . 

There are two possible ways to fix this problem. The first is to try and add heuristics to the 
projection process to restrict the possible combinations of clamped and undamped states 
(see the matlab code for some examples). This has not been successful so far. The second is 
to detect when we come back on a previously encountered index set and then alter the rules 
for index swapping to prevent us following the same path. This could possibly be done pre- 
emptively at the level of individual friction pyramids before the global index set has 
repeated. This has not been tried yet, it is difficult in matlab. 

There are several simple things that improve the speed of this algorithm. The first is finding 
a good starting index set, by applying the switching condition to every single index in one 
go. In fact this procedure may be followed for several iterations to improve performance, 
although it must be eventually abandoned for the one-at-a-time approach to ensure conver- 
gence. Also re-using index sets from the previous simulation iteration is useful. We need to 
investigate heuristics for choosing the switching index. For example, should we switch on 
the most or least violating index? 
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2.1.2 Tyres 

Tyres have special friction requirements. When the tyre is under longitudinal force, the fric- 
tion cone is actually scaled along one axis into a sort of friction ellipse. This can be simu- 
lated by altering the friction pyramid so that it is a kind of friction "diamond", and ensuring 
that the tangential x and y axes are aligned with the wheel. Tyres also have the characteris- 
tics of slip and pneumatic trail, which I have not investigated yet. 

3.0 More implicit integration 

It is possible to perform implicit integration on only some state variables in a system. For 
example, we can divide up the variables like this: 

y £ = explicitly integrated variables (3.1) 

y 1 = implicitly integrated variables (3.2) 

} .£ = /£(y£ yi) (3.3) 

yi =/ / (v £ } ,/) (3 .4) 

And we can integrate like this (Euler first order): 

>f*l = >'f + */W.v/) (3-5) 

yUx = y/+/»/'of + „>•/♦,) (3-6) 



CY 



(vf.y/)J 



• I 

/'(.vf,.v/) (3.7) 



More useful in rigid body dynamics is being implicit in some inputs to a system. Consider 
the system: 

y =/(>•, jc) (3.8) 
x = g(y) (3.9) 

Here / is the rigid body system, y is its state variables, and x is the "external" forces pre- 
sented to it. We can do implicit Euler integration only in the variables x as follows: 

y <+ , - y, + */(**.*/♦!> - (3.10) 

= «0',.i> <3-l >) 

if we make first order approximations for / and g: 
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/( y, x) */( y„ x;) + J F {x- x,) (3.i2) 

S(.V)*S(.V,) + ./ c (y-.y f ) (3.13) 

which gives 

>'i * i = .V, + (jr - J F J c ) /( v„ Xi ) (3. 1 4) 

If the external forces x are stiff, e.g. if they come from stiff springs, then this formulation 
will add extra stability to the system without the overhead of making the rigid body dynam- 
ics fully implicit. 

To implement this scheme we need to compute J F and J G . Computing J G should be rela- 
tively easy. To compute J F , first define the external force /, as 

/, = Qx (3.15) 

then 

d n \ d • 

-Wrsfwkx (3 - i? > 

= Af-'e-W-'y^M-'y^-'/M-'Q (3.| 8) 
which means that for the already factored system matrix (JM-'J 7 )^ we must solve for n, 
extra right hand sides in the matrix JM-*Q and then do'a few other cheaper operations. Thus 
we should be selective about which external forces get the implicit treatment (stiff forces are 
the obvious candidates). 



3.1 Modular Systems 

The construction of modular systems and the application of the chain rule to propagate 
jacobian information for the purposes of implicit integration are well known in the art. 
An example is provided in the MATLAB reference manual 



SUBSTITUTE SHEET (RULE 26) 



WO 01/67310 



PCT/GB01/01020 



41 

Appendix 2 



Contents 

1. Conventions 

2. Kea core 

2.1. Error handling 

2.2. Math stuff 

2.2.1. Miscellaneous 

2.2.2. Vector and quaternion stuff 

2.2.3. Simple matrix stuff 

2.3. Rigid body dynamics core 

2.3.1. Rigid body structure 

2.3.2. Rigid body functions 

2.3.3. Abstract constraint functions 

2.3.4. Worlds 

3. Constraints 

3.1. Contact 

3.2. Ball-and-socket 

3.3. Hinge 

3.4. Prismatic 

3.5. Steering hinge (1) 

4. Collision 

5. Utilities 

5.1. Kinematics 

5.1.1. Telescope segment 

5.1.2. Keep place 

6. Other 

6.1. Constraints 

6.2. Dynamics scheduler 

6.3. More functions 

6.4. Controller layer 



SUBSTITUTE SHEET (RULE 25) 



WO 01/67310 



PCT/GB01/01020 



42 

1. Conventions 

All matrices are stored in column-major order, i.e. stored by columns. 

2. Kea core 

2.1. Error handling 

The following three functions are used internally within Kea to generate error and warning 

messages: 

void ke = Fi-alError {char w ~sz, ...),- 

void keaDebug (char *msg, . ..); 

void keaWarning (char *msg, ,..); 
Each of these functions has an interface identical to print f ( ) , they take an error message 
string and a variable number of arguments. These functions can also be called by the user. 
The default behavior of these functions is as follows: 

• keaFatai£rror : print the error message to the console and exit. 

• kea Debu? : print the error message to the console, generate debugging information (e.g. 
dump core on a unix system) and exit. 

• keawarnir.g: print the error message to the console, and continue running. 
The default behavior can be overridden using these functions: 

typedef void keaErrorFunction (char *msg, ...); 

void keaSetFatalErrorHandler (keaErrorFunction *fn); 

void keaSecZabugHandier (keaErrorFunction *fn) ; 

void keaSetWarningHandler (keaErrorFunction *fn); 
It is useful to override the default behavior on systems without a text console (for example 
the PlayStation 2) or in shipping software. Note that the fatal error and debug calls are 
expected never to return, however they may perform exception handling using the C set- 
jmp() /longjmpO functions. 

2.2. Math stuff 

The typedef keaFioat is used everywhere in Kea as the floating point type. The constant 
kealnf inity is defined to correspond to the system infinity value, or if that does not exist 
the largest representable floating point value. 
2.2.1. Miscellaneous 

void keaSetZero (int n, keaFloar *A] ; 
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Set the first n elements of A to zero. For large arrays this will normally be faster than 
a simple for-loop. 

2.2.2. Vector and quaternion stuff 

keaFloat keaCot (keaFloat b[3], keaFloat c[3]); 

Return the inner product of b and c, assuming both are vectors of size 3. 

void keaCross (keaFloat b[3], keaFloat c(3], keaFloat a[3]); 

Set a to the cross product of b and c. assuming both are vectors of size 3. 

void keaPiar.eSpace (keaFloac n[2;, keaFloat a[3), keaFloat b[3]); 

Make unit length 3x1 vectors a and b such that together with the unit length 3x1 vec- 
tor n they form an orthonormal basis, a and b span the plane that is normal to n, and a 
x b = n. note that if n is not normalized then b will not be normalized either. 

void keaQProduct (keaFloat p[4], keaFloat q[4], keaFloat r[4]); 
Multiplication of quaternions: set r = p * q. 

void keaMakeUnitVector (keaFloat v[3]); 
Make the given size 3 vector unit length. 

2.2.3. Simple matrix stuff 

void keaMultiply (int p, int q, int r, keaFloat *B, keaFloat *C, keaFloat 
*A) ; 

Set A=B*C, where A is p*r, B is p*q, C is q*r. 

void kea.MultiplyTl (int p, int q, int r, keaFloat *B, keaFloat *C, keaF- 
loat *A) ; 

Set A=BT*C, where A is p*r, B is q*p, C is q*r. 
2.3, Rigid body dynamics core 
2.3.1. Rigid body structure 

The keaBody structure represents a rigid body in Kea. The coordinates of a rigid body 
(x,y,z) are always with respect to the body's center of mass. 

There are a number of internal variables that are made public for ease of access. You should 
not modify these directly! 
struct keaBody { 

void *usercata; 

keaFloat mass, I [9) ; 

keaFloat pes 1 3] , qrot ( 4 ) , vel [ 6] ; 

keaFloat R[ 9) ; 
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...other i.-.iernal stuff... 

) 

The body's mass parameters are mass and I, a 3x3 symmetric inertia tensor matrix. The 
body's state variables are 

• pos, the center of mass (COM) position (x.y.z). 

• qrot, the four quaternion rotation numbers. 

• vel, the COM velocity (vx,vy,vz). and the angular velocity (wx.wy.wz). 

R is the body 3x3 rotation matrix. It is a direct function of the qrot vector, and it is updated 
whenever qrot changes. 

userdata is a variable that the user is free to change, this is never used by Kea. 
2.3.2. Rigid body functions 

All rigid body functions can be called at any time between simulation timesteps. 

void keaBodylnitialize (keaBody *body) ; 

Initialize the body. This must be the first function called on a new body. 

void keaBody.--tach (keaBody "body, keaWorld *world) ; 

Attach the body to the world, making it an active part of that world. A body must be 
attached to a world before it can have constraints attached to it. If the body is already 
attached to another world it will be detached from that world first. If the body is 
already attached to the world then nothing will be done. 

void keaBodyletach (keaBody *body) ; 

Detach the body from whatever world it is currently attached to. Any constraints that 
are connected to this body are disconnected first. This does not destroy any body data, 
it simply prevents the body from being a part of the simulation. The body can be re- 
attached at any time. 

Now here are some functions to set the mass distribution of the body. 

void keaBodyvakeSphere (keaBody *body, keaFloat mass, keaFloat radius); 
Set the mass parameters of this body to a sphere of the given mass and radius. 

void keaBodyXakeBox (keaBody 'body, keaFloat mass, keaFloat lx, keaFloat 
ly, keaFloa- iz) ,- 

Set the mass parameters of this body to a box of the given mass and side lengths. 

Now here are some functions to set the position, rotation, and velocity of the body. If you set 

values that are inconsistent with the current constraints then the simulation will attempt to 

correct this in subsequent time steps. 
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void keaBodySetPosition 

(keaBody *body, keaFloac x, keaFloat y, keaFloat z); 
void keaBodySetQuaternion 

(keaBody 'body, keaFloat ql, keaFloat q2, keaFloat q3, keaFloat q4 ) ; 
void keaBodySetLinearVelocity 

(keaBody ~bcdy, keaFloat dx, keaFloat dy, keaFloat dz); 
void keaBodySetAngularVelocity 

(keaBody 'b cdy, keaFloat wx, keaFloat wy, keaFloat wz); 
Now here are some functions to add forces to the body. After each time step the body is 
assumed to have zero force acting on it. These functions accumulate force on the body for 
the next time step, 
void keaBocyAddForceAbs 

(keaBody 'tody, keaFloat fx, keaFloat fy, keaFloat fz); 
void keaBocy.-.ddForceRel 

(keaBody -body, keaFloat fx, keaFloat fy, keaFloat fz); 

Add a force, in the absolute (inertial) frame or the relative (body) frame, to the body's 
center of mass, 
void keaBodyAddTorqueAbs 

(keaBody 'body, keaFloat tx, keaFloat ty, keaFloat tz); 
void keaBocyAddTorqueRel 

(keaBody 'body, keaFloat tx, keaFloat ty, keaFloat tz); 

Add a torque, in the absolute (inertial) frame or the relative (body) frame, to the 
body's center of mass. 
2.3.3. Abstract constraint functions 

The keaConstraint structure represents a one, two or three body constraint. The constraint 
services described below are used by all the system joint types, and allow new constraint 
types to be created by the user. Note that all variables in the keaConstraint structure are 
internal and should not be accessed directly. 

The following constraint functions can be called at any time in the simulation, 
void keaConstraintlnitialize (keaConstraint +c) ; 

Initialize the constraint. This must be the first function called on a new constraint, 
void keaCor.straintAttach (keaConstraint *c, keaWorld *world) ; 
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Attach the constraint to the given world, making it an active part of that world. If the 
constraint is attached to another world, it will be detached from that world first. If the 
constraint is already attached to the world then nothing will be done, 
void keaCor.s-raintDetach (keaConstraint *c) ; 

Detach the constraint from whatever world it is currently attached to. This does not 
destroy any constraint data, it simply prevents the constraint from being a part of the 
simulation. The constraint can be re-attached at any time. 

void keaCor.s-raintSetBodies 

(keaConstraint *c, keaBody *bl, keaBody *b2, keaBody *b3); 

This set the bodies that the constraint attaches. The constraint must have been 
attached to a world, and the bodies must be in the same world. 
keaConstrai.-.-Define (keaConstraint *c, int num_ce, keaGetlnfoFn 'getinfo, 

keaStartStepFn *startstep) ; 

Sets the behavior of the constraint. This is only called to define new constraint types. 
num_ce is the number of constraint equations, getinfo and startstep are pointers to 
functions that implement the constraint behavior, getinfo gets information about this 
constraint for the current state of the constrained bodies, startstep is called auto- 
matically at the start of each timestep, it can set some auxiliary state-based data (such 
as joint angles) which the user can read. If you change the state of the constraint or the 
bodies which it connects then you may call this function yourself to update that data. 
Arguments to the getinfo function are provided in a structure rather than being 
passed directly, to allow for future expansion without having to rewrite all the con- 
straint code. The getinfo function is free to ignore any of the arguments in this struc- 
ture, except for the essential 'J'. Each matrix/vector here has num_ce rows to fill in. 
struct keaCor.straintlnfo ( 

kea Float *-";3] ; 

int rows kip; 

kea Float -c; 

keaFloat -y.i; 

keaFloat ~ lower, * upper ; 

keaFloat "slipf actor; 

}; 

-ypedef void KeaGetlnfoFn ( keaConstraintlnfo *); 
The structure members are: 
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• J: Up to 3 pointers to Jacobian matrices that must be filled in. These matrices are stored 
by rows for convenience in constraint formation, in contrast to the usual Kea storage 
convention. 

• rowskip: How much to jump by to go between J matrix rows. 

• c: Vector in the constraint equation J*v=c. 

• xi: Constraint error vector. 

• lower, upper: Lagrange multiplier limits. 

• slipfacror: First order constraint slipping vector. 
2.3.4. Worlds 

The iceaworld structure represents a simulated world in Kea. All members of this structure 

are internal and should not be accessed directly. 

void keaWorldlnitialize (keaWorld *world) ; 

Initialize a new world. After initialization, bodies and constraints can be attached to it. 

void keaWoridDestroy (keaWorld *world) ; 

Destroy the given world. This simply detaches all bodies and constraints from the 
world, emptying it. 

void keaWorldAddGravity (keaWorld *worid, keaFloat gravity); 

Add a downwards (-z) gravity force to all bodies in the world, gravity is given in m/s2. 

void keaWoridSuepl {keaWorld *world, keaFloat stepsize); 

Evolve the world forward in time by stepsize seconds. This uses an algorithm 
which will be fast, except for systems containing large articulated rigid body struc- 
tures. 

void keaWorldSetEpsilon (keaWorld *world, keaFloat x) ; 

void keaWorldSetGamma (keaWorld *world, keaFloat x) ; 

These functions set world parameters. Increasing epsilon helps to combat numerical 
instability problems caused by degenerate systems. Increasing it will make the simu- 
lation more "non-physical" but may smooth over simulation glitches. The default 
value is 0.0001, increasing this to 0.1-1 will result in observable non-physical effects 
for worlds where that masses are on the order of 1kg. 

Gamma is the projection constant which controls constraint stabilization. If the rigid 
body configuration has diverged from its constrained configuration, the next time step 
it will be brought a fraction 'gamma' of the way back to its correct configuration. Set- 
ting gamma to zero will result in articulated structures gradually coming apart. Setting 
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gamma to one and higher will result in instabilities as the simulation tries to "over cor- 
rect". The default value is 0.2, and that is probably good enough for most simulations. 

3. Constraints 

To make a constraint, call its initialization functions, and then call the keaConstraint At- 
tach () and keaConstraintSetBodies() functions. 
3.1. Contact 

The keaContact constraint is a collision contact between body 1 and body 2, or a collision 
contact between body 1 and the static environment. The keaContact structure has a number 
of public variables that must be set before the world step function is called: 

struct keaContact { 
int mode; 
keaFloat epos [3] ; 
keaFloat normal [3] ; 
keaFloat penetration; 
keaFloat max_force; 
keaFloat a [3] ; 
keaFloat kl; 

...other internal stuff... 

}; 

The fields of keaContact are: 

• mode: The contact mode is one of the following constants: 

• 0: Zero friction. 

• kea t friction2d: Friction in two directions using automatically determined 

principal directions. 

• kea_friction_id: Friction in one direction (vector a). 

• kea_friction_tyrei: Friction in wheel drive direction (vector a) and first order 

slip in the lateral (other tangential) direction. The slip factor is the value kl The 
following flags can be ORed with mode: 

• kea__friction_box: Friction force magnitude along each principal direction is 

limited to max_force. 

• epos: The contact position, in absolute coordinates. This must always be set. 

• normal: The vector that is normal to the contact sliding plane, relative to body 1. This 
must have unit length. This must point 'in' to body 1, that is body 1 motion is allowed 
along the direction of +normal and body 2 motion is allowed along the direction of -nor- 
mal. This must always be set. 
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• penetration: The penetration distances of the contact along the normal direction. This 
must always be set. 

• max_f orce: The maximum factional force that can be applied. This is set when the box 
friction flag is set. 

• a: Parameter vector a. This is set in kea_friction_id and kfa_friction_tyrei 
modes. It must have unit length. 

• fci: Parameter kl. This is set in kia_frictiom_tyrsi mode. 
The keaConcar- functions arc: 

void keaCon- art Initialize ( keaCor.tact 'contact); 
Initialize a new contact. 

3.2. Ball-and-socket 

The keaBS Joint structure represents a ball and socket joint, 
void keaBSJoint Initialize (keaBSJoint * joints- 
Initialize a new ball and socket joint. 
keaBSJointSe- Position (keaBSJoint *joint, keaFloat x, keaFloat y, keaF- 
ioat z) ; 

Set the joint position (in absolute coordinates). The constraint bodies must have been 
set first, and the positions of the joined bodies must have been set. 

3.3. Hinge 

The keaHinge structure represents a hinge joint. Note that the initial position of the hinge ; 

will be taken as the zero reference for angle determination. 

void keaHingelnitialize (keaHinge *joint); 
Initialize a new hinge joint. 

void keaHingeSetPosition 

{keaHinge -joint, keaFloat x, keaFloat y, keaFloat z) ; 

void keaHingeSetAxis 

(keaHinge * joint, keaFloat x, keaFloat y, keaFloat z) ; 

Set the position of the hinge joint and its axis (in absolute coordinates). The joint bod- 
ies must have been set first, and the positions of the joined bodies must have been set. 

void keaHingeSetNoLimits (keaHinge "joint) ; 

void keaHir.geSetLimits (keaHinge 'joint, keaFloat low, keaFloat high); 

Set joint limits, low and high are in radians and are relative to the zero n*f,rence 
determined by the initial position of the hinge. 
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void keaHingeSetNoMotor (keaHinge 'joint); 

void keaHingeSetLimitedForceMotor (keaHinge * joint, keaFloat 
desired_velocity, keaFloat force_limit ) ; 

Sets a motor on the hinge. 
keaFloat * keaHingeGetAxisAbs (keaHinge * joint); 

Returns a pointer to a size 3 vector which gives the current hinge axis in absolute 

coordinates. 

keaFloat keaHingeGetAngle {keaHinge "joint); 
keaFloat keaHmgeGeiAng legate (keaHinge + joint); 

Returns the current hinge angle and angular velocity. 
3.4. Prismatic 

The keaPrism structure represents a prismatic joint, 
void keaPrisr.Initialize (keaPrism *joint); 
Initialize a new prismatic joint. 

void keaPrisr.SetAxis (keaPrism 'joint, keaFloat x, keaFloat y, keaFloat 
z) ; 

Set the sliding axis for the prismatic joint (in absolute coordinates). The joint bodies 
must have been set first, and the positions of the joined bodies must have been set. 
void keaPrismSetNoLimits (keaPrism *joint); 

void keaPrismSetLimits (keaPrism *joint, keaFloat low, keaFloat high); 

Set joint limits, low and high are in meters - poskion zero is when the centers of mass 

of the two bodies are as close to each other as possible, 
void keaPrismSetNoMotor (keaPrism *joint); 

void keaPrismSetLimitedForceMotor (keaPrism * joint, keaFloat 
desired_velocity, keaFloat f orce_limit ) ; 

Sets a motor on the joint. 
keaFloat * keaPrismGetAxisAbs (keaPrism *joint); 

Returns a pointer to a size 3 vector which gives the current sliding axis in absolute 

coordinates. 

keaFloat keaPrismGetPosition (keaPrism *joint); 
keaFloat keaPrismGetPositionRate (keaPrism *j'dlnt); 

Returns the current sliding position and velocity. Position zero is when the centers of 

mass of the two bodies are as close to each other as possible. 
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3.5. Steering hinge (1) 

The keaSceerir.gKinge structure represents a steering hinge joint, which is a hinge that can 
be steered along a steering axis. This joint is useful on the front wheels of cars. Body 1 is 
the chassis and body 2 is the wheel. The connection point for the wheel body is its center of 
mass. 

void keaS-eeringHingelni tialize ( keaSteeringHinge *joint); 

Initialize a new steering hinge joint, 
void keaSreeringHingeSetSteeringAxis 

( keaSteeringHinge *joinc, keaFloat x, kea Float y, keacloac z ) ; 
void keaSneeringHingeSetHingeAxis 

(keaSteeringHinge * joint, keaFloat >:, keaFloat y, keaFloat z) ; 

These functions set the joint geometry, the steering axis and the hinge axis. The joint 

bodies must have been set first, and the positions of the joined bodies must have been 

set. 

keaFloat * keaSteeringHingeGetHingeAxisAbs (keaSteeringHinge *joint); 
keaFloa- - keaSteeringHingeGetSteeringAxisAbs (keaSteeringHinge * joint); 

Returns pointers to size 3 vectors which give the current hinge and steering axes in 

absolute coordinates. 

4. Collision 

The Kea collision API is in a state of flux and will net be documented here yet. Bui check 
out the source file kea_collice.h if you want to know what the current story is. 

5. Utilities 
5.1. Kinematics 

These functions allow for easy kinematic (rather than dynamic) placement of objects. They 
are specific to particular kinematic situations that the author has come across in the past, so 
not all common cases are covered here! 
5.1.1. Telescope segment 

The keaKir.e.T.aticTeiescopeSegment structure and associated functions allow kinematic 
placement of an intermediate telescope segment (body 3) given the positions of the seg- 
ments at the ends of the telescope (body 1 and body 2). 
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void keaKinematicTelescopeSegment Initialize ( keaKinemat icTelescopeSeg- 
ment *k, 

keaFloat pcsl [3] , keaFloat R[9], keaFloat pos2[3], keaFloat pos3[3]); 
Initialize a keaKinematicTelescopeSegment structure, giving the initial positions of 
the three bodies, and the rotation matrix of body 1. 
void keaKine-aticTelescopeSegmentGetPosition ( keaKinemat icTelescopeSeg- 
ir.ent *k, keaFloat posl[3], keaFloat R[9] ( keaFloaz pos2[3], keaFloat 
pos3[3]); 

Given the positions of bodies 1 and 2, and the rotation matrix of body I, return the 
position of body 3. 
5.1.2. Keep place 

The keaKinema-icKeepPiace structure and associated functions allow kinematic place- 
ment of a body (number 2) always in the same place relative to another body (number 1). 
void keaKinen.sticKeepPlacelnitialize ( keaKinematicKeepPlace *k, keaFloat 
posl[3], keaFloat R[9], keaFloat pos2 [3] ) ; 

Initialize a keaKinematicKeepPlace structure, giving the initial positions of the bod- 
ies, and the rotation matrix of body 1. 

void keaKinerr.aticKeepPlaceGetPosition {keaKinematicKeepPlace *k, keaFloat 
posl[3], keaFloat R[9], keaFloat pos2[3]); 

Given the position and rotation matrix of body 1, return the position of body 2. 

6. Other 

Some parts of the Kea API are not covered here, mostly those parts that haven't even been 
designed yet! Here are notes about what is missing. 
6.1. Constraints 

More constraint types are needed, especially the linear-1, angular-1 stuff from the SDK. 
This will be trivial to add. 

Document the functions that help implement user defined constraints, e.g. getting the bodies 
a constraint attaches. 

For the contact constraint, we are currently missing: 

* velocity dependent slip. 

* the friction cone one-step-late approximation. 



SUBSTITUTE SHEET (RULE 26) 



WO 01/67310 



PCT/GB01/01020 



53 

* keep the physical stuff in separate structure, i.e. separate the physical quantities from the 
geometrical ones. 

Take a look at how well the keaConstraint functions operate on the joint types - is there an 
annoying type casting issue? 

6.2. Dynamics scheduler 

This things hasn't even been designed yet. It could sil outside the Kea core, in which case 
we must check how to detach groups of RBs and constraints from the world without 
destroying the relationships between then, so they can be attached again later. 

6.3. More functions 

Need more functions to compute rotations, e.g. to set body orientation. Use 
functions from glowworm. Open source. 

Need more functions to add force to bodies, e.g. at a non-COM position. 
Again, use the functions from glowworm. Open source. 

6.4. Controller layer 

An open source dataflow based control layer, that allows us to easily implement 'gadgets' 
such as springs, PD controllers etc. Issues: data transfer (wiring), encapsulation of basic 
Kea structures, driving of the simulation. This should be quite easy. 
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Appendix 3 

Stiff Spring Simulation 

from notes by Russell Smith 



This describes how 10 implement a stiff spring in Kca using constraints. The kev is to allow 
first order slip along the spring direction. Consider a one dimensional sinsle particle system 
(mass m): 

P = " (3.1) 
- ;V + ^> (3.2) 

where p is the point position, v is its velocity, / is the external force and X is the constraint 
force (all scalars). We will enforce the constraint p = 0 by setting J to 1 . Thus from equa- 
tion 1.80 in appendix 1, k is computed as: 

Km J h 2 h m (33) 

so the semi-implicit update for p and i« is: 

v ul « Vi +tL(f+\) (34) 

- ii *. h f u ; "" "' " ' 2 

Ml 



_ „ , r, /[E /71£ _y 

- ^ + /(rT^) + KiT^) + P(,7(r^F)) (3.7) 



and 

i = P, + hv i+l (38) 

Now consider removing the constraint, adding an external spring and damper force, and 
integrating implicitly: 

/. = - k pPi-k d V; (3.9) 

= 0 (3.10) 

Pi* i = /', + /'»•,*, (3.H) 
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m 



= ^ + £(-V-|-V'/+.) (313) 



= + V *,(Pi + .) " * rf v, + ,) (3.14) 



(3.15) 



(. M p + M j\ hk 

^ ' = v L + h"L + hJ +p L + Jt p + hkJ (3 - ,6) 



We can equate coefficients with equation (3.7) to see how to construct an "implicitly inte- 
grated spring" with a constraint (take / to be zero in equation (3.7)). First equate the coeffi- 
cient on v, to find e : 



me 



in 



1+me m +h 2 k p + hk d 



(3.17) 



h% + hk d 

Then equate the coefficient on p t to find y : 

-Y -hk p 



e{m + h 2 k p + hk d ) = 1 +me (3.18) 
e = — — ■ (3.19) 



/i(l+me) m+h'-kp + hk,, 
h 2 k p ( I + me ) 



(3.20) 



y = — - (3.21) 

in + /i 2 fc + hk d 



hk„ 



hk + k d 



(3.22) 



To summarize, 



e = - T — ■ (3.23) 

h 2 k„ + hk. 



T = hk~+Z (3 " 24) 



= e/i 2 A p (3.25) 

The parameters e and y are both dependent on the time step, but they are not dependent on 
the mass, so this constraint functions as a true spring. When e is 0 this corresponds to a 
spring or damper constant of infinity, which results in an unmovable spring (complete con- 
straint satisfaction). 
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Mz + q = ir 
:,>0 
if, > 0 

W,Z: = 0 



(4.27) 
(4.28) 
(4.29) 



Appendix 4 

4.1 Linear Complementarity Problems 

Given a real square n x n matrix M and a real ii-dimcnsional vector q, the complementarity 
problem consists of finding the H-dimensional vectors • and w such that they satisfy the fol- 
lowing conditions: 

A A - , .. _ ... 

(4.26) 

Vie (1,2,...,/,) 

Vie |l,2 n| 

Vie {1,2, ...,„) 

This is fundamentally a combinatorial problem and various direct algorithms are available 

where the search goes through index sets a, p such that ct c { 1,2 u) , p c { 1,2 n) , 

a n p = 0,aop = { 1, 2 n) . The set a is the set of active variables and w { = 0 Vi e a 

while the set p is the set of free variables such that z , = 0 Vi e p . The problem is then par- 
titioned as: 



(4.30) 



where the subscripts a, p, are used to specify all indices that are in the sets a, p respec- 
tively. This is equivalent to the linear algebra problem: 

M uaZa ~ -Qa ~ (4.31) 
u '[> = M ^u + <7p (4.32) 

which must be solved for z a while w p is computed by direct substitution. The matrix M aa 
is known as a principal submatrix of the matrix M. Various principal pivoting algorithms 
perform the same basic computation but take different strategies to revise the sets a and p 
from the computed complementarity point which is a vector s such that: 

z,- Vi e a 
w.. Vi e P 









7 


+ 






0 








0 




fa 







S: = 



(4.33) 



All principal pivot methods go through a sequence of sets.a,, p„ i = 1,2,... until a solution 
is found i.e., i,>0Vie (1,2, ...,«} . 
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4.2 The Murty Algorithm 

One important such algorithm is the Murty Principal Pivot Method which is as follows: 

1 . Initialize (basically, any guess for ct (0) is good), set / = 0 . 

2 . form principal submatrix M a( „ a <,> and solve Af oWa „,x aW = -<7 a </> 

3 . compute s<" = s u <o + w p(i , where W P W = w p«o<« 2 oW 
4. if ^' l >0 V y we arc done 

else 

find the smallest j such thai sj i] < 0 , 
if = z ; then ct<' +| 5 = a<'>\{./} 
else a(' + l > = a^u {;} 
/<-/+! 
goto step 2 

This algorithm is special in that it is stateless and can be started from any initial guess for 
a 0 . This method will work on any P matrix and in particular, any positive definite matrix. 

The flowchart for this algorithm is given in figure 1 below. 



43 The Kostreva Perturbation Method 



In the case where the matrix M is positive semi-definite, the Murty principal pivot method 
can fail. This can arise in a multi-body simulation in the case where the constraints are 
degenerate, or if we work from the optimization matrix which leads to the form: 



(4.34) 

(4.35) 
(4.36) 



N 






V 


+ 


-F 




0 


J 


0_ 




\ 




c 




)• 



X > 0 v > 0 
\'v = 0 



for a multibody problem with inequality constraints. This matrix is positive semi-definite if 
the mass submatrix /V is positive definite (which is always the case for physical systems). 
However, given any number e > 0 , the matrix obtained from the original one by adding e on 
the diagonal is always positive definite, i.e., the matrix: 



E/i 



(4.37) 



is positive definite, where / /Y and / y are identity matrices of appropriate sizes. Kostreva 
demonstrated that one can solve the sequence of complementarity problems defined by a 
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sequence of positive numbers {e M }- = , such that e„ -+ 0 as n - «, and find an answer to the 
positive semi-definite problem or find that it is infeasible (i.e., there is no answer). The algo- 
rithm is as follows; 

1. Choose e(°)>0 (typically 10" 6 ) 

2. Solve LCP(M< e °>,q) 

3 . set /'<-/+ i . choose e<'> < s*' - 1 » 

4. Solve LCP(M<<\ q) 

5. if k^'-zV-VUtol Solve LCP(M, q) and finish 

6 . else if || z (0 - ?«- D|| < max goto step 2 
7 . else ciTor: problem is infeasible 

We often set c«>> = \ 0 -o and E <» = 0. This is often sufficient. In Kea, we even go further 
and set e<°> = i(H and stop right away i.e., we don't bother removing the perturbation. 



4.4 Boxed LCPs 



The boxed LCP problem starts from the definition of the standard LCP and adds two new n- 
dimensional vectors / and u, lower and upper bounds respectively, such that 
/, <w, V/e {1,2, . and then the problem reads: 



Mz + q = u + 






(4.38) 


2,-/^0 


V/ € { 1, 2, . 


...if) 


(4.39) 


>. + ,.>o 


We {1,2,. 


...») 


(4.40) 




V/6 {1,2,. 


..,») 


(4.41) 




V/€ (1,2,. 


..,«) 


(4.42) 


»•.,> o 


Vis {1,2,.. 


.,«} 


(4.43) 


(«*-«/)»"♦/ = o 


Vie {|,2,. 


.,«) 


(4.44) 



This is equivalent to a much larger mixed linear complementarity problem defined 



as: 



My. 5 0 



M -I I 




7 




<7 




0 


I 0 0 








-/ 






-/ 0 0 




vv_ 




rr 




V 


= 


0 






W_; > 0 



V,M'., = 0 



(4.21) 



4.22) 
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This is precisely the son of problem that the Kostreva procedure is designed to solve. How- 
ever, because of the structure of the problem, there are simplifications that can be achieved 
on this. The idea is to partition the set p defined as above into two sets, y and i so that 
z . = / for j € v and Zj = Uj for j e i . Afterwards, we follow the Muny algorithm but we 
change the definition of the complementarity point s as follows: 

min(zj-lpUj-ij) \fjea 
(M, a z u - M r ,l y - M yx it x + q 7 )j Vj e y (4.23) 



S J = 



and then, the least index rule is applied to this new complementarity point as follows: 
j = min argO ; . < 0) 

if /€Ct<'> and 2 j0</ / then a< f+ 1 > = a<'My}, Y (,+ n - Y (0 ^ Uh^* 0 = l(0 
if jeaW and zj°>"; then ct< l + I > = a('A{y}.i ( ' +l) - U}>Y (, + l) = Y (/) 
if ;ey<'*> then a f +l > = o«u 0'}, y^* 1 ) = yW\{y} B t a+ D = t (0 
if jeyf* then a<' + » = a<"u {;}, i« + l > = i<'>\0}, Y (/ + ! > = y (,) 

This modification of the standard Murty algorithm has not been traced in the literature by 
the authors yet. 



4.5 Box Friction 

The Coulomb friction model specifies the tangential forces at a point of contact in terms of 
the normal force at that point. The model specifies non-ideal constraint forces i.e., forces 
arising from constraints that do work on the system. This is in sharp contrast to typical ideal 
constraints which do no work on the system. Coulomb friction implements a maximum dis- 
sipation principle i.e., when the velocity at the point of contact is non-zero, the tangential 
force will be aligned against the velocity in a way that maximizes the work done; for isotro- 
pic friction, this means that the force of friction is directly opposed to the velocity of the 
contact point in the contact plane. One should note that Coulomb friction is a constitutive 
law i.e., an empirical model which is meant to summarize experimental evidence, in con- 
trast with a fundamental law which can be derived from first principles. As such, "Cou- 
lomb's Law" is not so strict: the modeler has license to aker this model to ease computation 
provided the overall behaviour is still close to what is seen in an experiment. 

The Coulomb friction model for a single point describes two states for the contact namely, 
sticking or sliding. In stick mode or static friction mode, the tangential force vector, f t , 
which lies in a plane tangential to the normal force of contact, the force that prevents two 



SUBSTITUTE SHEET (RULE 26) 



W0 01/67310 PCT/GB01/01020 



60 

objects from interpenetrating, must have smaller magnitude than the friction coefficient \i s 
times the magnitude of the normal force f n i.e., 

fi\\ = JfK+f&ZvJ* (4.24) 



where /,, and are components of the tangential force along two perpendicular directions 
in the tangential plane. In dynamic or sliding friction, the friction force must oppose the 
sliding velocity vector v t and its magnitude is the kinetic friction coefficient \i k times the 
normal force i.e. 

||/, || = VJn (4.25) 

ft = />,] +/i2 v f2 s0 W.26) 

This model does not specify how to compute f H or f t at all but only states relationships 
between those quantities. These conditions specify that the tangential force should lie in the 
convex cone defined by the normal force, i.e., 

C W = | ||/,i<M/„} (4.27) 

The first approximation we perform on this is to replace the friction cone by a friction pyra- 
mid, a simple case of polygonal cone approximation found in the literature. The idea is to 
introduce k linearly dependent basis vectors for the contact plane denoted rf h d 2l d k and 
to represent the tangential friction force as: 

ft = £y,P, (4.28) 

i 

and from this definition, we get an approximation to the friction cone known as the friction 
polyhedron: 



°<IWJ/"J 



(4.29) 



The construction is shown for the friction pyramid in figure 4 using four basis vectors with 
equal angular spacing. 

The final approximation is to neglect the dependence of the tangential friction forces on the 
normal force by specifying an independent maximum value for the coefficients, / max . This 
gives a simple approximation off the friction cone that we refer to as a 'box friction' model. 



0<P,<-/ ma x| (4.30) 
A box friction approximation to the friction cone is shown in figure 5. 
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Claims 

1. A computer program recorded on a data carrier for 
simulating the motion of objects and displaying the results on 
a display screen, the computer program being operable to 
control a computer having a display screen, a memory and a 
processing unit to carry out the steps of 

storing in the memory position and velocity parameters 
defining an initial state of a model system having a plurality 
of bodies, 

storing in the memory parameters defining at least one 
constraint function constraining the motion of the bodies in 
the model system, and 

calculating in the processor the position and velocity 
parameters defining the state of the system after a 
predetermined time step based on rigid body dynamics, including 

carrying out a semi-implicit integration step subject to 
the constraints, to determine the velocity after the step, 
including 

determining the constraint forces that act to keep the 
system in compliance with the constraints by ensuring that the 
first derivative of the constraint function is zero. 

2. A computer program recorded on a data carrier for 
simulating the motion of objects and displaying the results on 
a display screen, the computer program being operable to 
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control a computer having a display screen, a memory and a 
processing unit to carry out the steps of 

storing in the memory position and velocity parameters 
defining an initial state of a model system having a plurality 
of bodies, 

storing in the memory parameters defining at least one 
constraint function constraining the motion of the bodies in 
the model system, 

storing in the memory parameters defining a bounded 
constraint force to simulate the effects of friction in which 
the constraint force acts in the plane of contact between a 
pair of objects to prevent sliding of one of the pair of 
objects over the other of the pair, wherein the constraint 
force is bounded to be not greater than a predetermined 
constant value to allow sliding of the objects over one another 
and thus include dynamic friction in the simulation, and 

calculating in the processor the position and velocity 
parameters defining the state of the system after a 
predetermined time step based on rigid body dynamics, including 

carrying out a semi-implicit integration step subject to 
the constraints, to determine the velocity after the step. 

3. A computer program according to claim 1 or 2 operable 
to cause the computer to carry out the further step of 
displaying an image of the objects at their calculated 
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positions on the computer display screen, so that the display- 
shows the objects on the screen using physical laws to simulate 
their motion. 

4. A computer program according to any preceding claim 
wherein the calculating step includes carrying out the implicit 
integration by 

calculating the velocity parameters after the time 
step from the external forces, the constraint forces and 
the position and velocity parameters before the time step, 
and 

calculating the position parameters after the time 
step from the external forces and constraint forces, the 
calculated velocity parameters after the time step and the 
position parameters before the time step. 

5. A computer program according to any preceding claim, 
wherein the constraint forces are determined by solving the 
mixed linear complementarity problem using Murty's method. 

6 . A computer program according to claim 5 wherein the 
means for solving the linear complementarity problem includes 
solving the boxed LCP problem by the boxed Murty's method. 
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7. A computer program according to claim 5 or 6 wherein 
the constraints are required to be held to within a tolerance c 
where the tolerance c has a predetermined value that is small. 

8. A computer program according to claim 7 where 6 has a 
value between 10" 4 and 10" 2 . 

9. A computer program according to any preceding claim 
wherein the model includes a model of friction in which the 
frictional force between a pair of objects is independent of 
the normal force between the objects. 

10. A computer program according to claim 9 wherein the 
frictional force between a pair of objects is modelled as a 
bounded constraint force in which the constraint force acts in 
the plane of contact between the pair of objects to prevent 
sliding of one of the pair of objects over the other of the 
pair, wherein the constraint force is bounded to be not greater 
than a predetermined constant value to allow sliding of the 
objects over one another and thus include dynamic friction in 
the simulation. 

11. A computer program according to any preceding claim 
wherein the bounds on the constraint forces are included by a 
step of testing whether the constraint forces have a magnitude 
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greater than a predetermined value and if so setting them to be 
that predetermined value. 

12. A computer program according to any preceding claim 
5 in which the constraints are modelled using the first order 

expansion of the constraint function <P : 

O = 4o + J P (p* -p) 
in the constraint equation. 

♦o + J P (P' - P) = fl-Y)*o 
10 where y is a relaxation parameter. 

13 . A computer program according to claim 12 when 
dependent on claim 6 wherein the parameters y and e are chosen 
to model a compliant coupling between two of the rigid bodies. 

15 

14. A computer program according to any one of claims 1 
to 13 that is a computer game program. 

15. A computer game program according to claim 14 
20 recorded within a cartridge for a computer game machine. 

16. A computer game programmed to generate a display by 
means of a computer program according to any preceding claim. 
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17. A method for operating a computer having a display 
screen, a memory and a processing unit for simulating the 
motion of objects and displaying the results on the display 
screen, the method including the steps of 

storing in the memory position and velocity parameters 
defining an initial state of a model system having a plurality 
of bodies, 

storing in the memory parameters defining at least one 
constraint function constraining the motion of the bodies in 
the model system, and 

calculating in the processor the position and velocity 
parameters defining the state of the system after a 
predetermined time step based on rigid body dynamics, including 

carrying out a semi-implicit integration step subject to 
the constraints, to determine the velocity after the step, 
including 

determining the constraint forces that act to keep the 
system in compliance with the constraints by ensuring that the 
first derivative of the constraint function is zero. 

18. A method for operating a computer having a display 
screen, a memory and a processing unit for simulating the 
motion of objects and displaying the results on the display 
screen, the method including the steps of 
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storing in the memory position and velocity parameters 
defining an initial state of a model system having a plurality 
of bodies, 

storing in the memory parameters defining at least one 
5 constraint function constraining the motion of the bodies in 
the model system 

storing in the memory parameters defining a bounded 
constraint force to simulate the effects of friction in which 
the constraint force acts in the plane of contact between a 
10 pair of objects to prevent sliding of one of the pair of 
objects over the other of the pair, wherein the constraint 
force is bounded to be not greater than a predetermined 
constant value to allow sliding of the objects over one another 
and thus include dynamic friction in the simulation, and 
15 calculating in the processor the position and velocity 

parameters defining the state of the system after a 
predetermined time step based on rigid body dynamics, including 

carrying out a semi -implicit integration step subject to 
the constraints, to determine the velocity after the step. 

20 

19. A method according to claim 17 or 18 including the 
further step of displaying an image of the objects at their 
calculated positions on the computer display screen, so that 
the display shows the objects on the screen using physical laws 
25 to simulate their motion. 
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20. A method according to any of claims 17 to 19 wherein 
the calculating step includes carrying out the implicit 
integration by 

calculating the velocity parameters after the time 
step from the external forces, the constraint forces and 
the position and velocity parameters before the time step, 
and 

calculating the position parameters after the time 
step from the external forces and constraint forces, the 
calculated velocity parameters after the time step and the 
position parameters before the time step. 

21. A method according to of claims 17 to 20, wherein the 
constraint forces are determined by solving the mixed linear 
complementarity problem using a Murty algorithm . 

22. A method according to claim 21 wherein the linear 
complementarity problem is solved by solving the boxed LCP 
problem by the boxed Murty' s method. 

23. A method according to claim 21 or 22 wherein the 
constraints are required to be held to within a tolerance e 
where the tolerance e has a predetermined value that is sir«.ll. 
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24. A method according to claim 23 where e has a value 
between 10" 4 and 10" 2 . 

25. A method according to any of claims 17 to 24 wherein 
the model includes a model of friction in which the frictional 
force between a pair of objects is independent of the normal 
force between the objects. 

26. A method according to claim 25 wherein the frictional 
force between a pair of objects is modelled as a bounded 
constraint force in which the constraint force acts in the 
plane of contact between the pair of objects to prevent sliding 
of one of the pair of objects over the other of the pair, 
wherein the constraint force is bounded to be not greater than 
a predetermined constant value to allow sliding of the objects 
over one another and thus include dynamic friction in the 
simulation. 

27. A method according to any of claims 17 to 26 wherein 
the bounds on the constraint forces are included by a step of 
testing whether the constraint forces have a magnitude greater 
than a predetermined value and if so setting them to be that 
predetermined value. 
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28. A method according to any of claims 17 to 27 in which 
the constraints are modelled using the first order expansion of 
the constraint function d> : 

<& = <|>o + J P (p ' -p) 
in the constraint equation. 

<t>o + J P (p' - p) = (l-y)0 o 

where y is a relaxation parameter. 

29. A method according to claim 28 when dependent on 
claim 19 wherein the parameters y and s are chosen to model a 
compliant coupling between two of the rigid bodies. 

30. A method of generating a display in a computer game 
according to any one of claims 17 to 29. 

31. Apparatus for simulating the motion of objects and 
displaying the results on a display screen comprising 

a display screen, 
a memory, 

a processing unit, 
and a computer program stored in the memory for causing the 
apparatus to carry out the steps of: 

storing in the memory position and velocity parameters 
defining an initial state of a model system having a plurality 
of bodies, 
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storing in the memory parameters defining at least one 
constraint function constraining the motion of the bodies in 
the model system, and 

calculating in the processor the position and velocity 
5 parameters defining the state of the system after a 

predetermined time step based on rigid body dynamics, including 

carrying out a semi - implicit integration step subject to 
the constraints, to determine the velocity after the step, 
including 

10 determining the constraint forces that act to keep the 

system in compliance with the constraints by ensuring that the 
first derivative of the constraint function is zero. 

32. Apparatus for simulating the motion of objects and 
15 displaying the results on a display screen comprising 
a display screen, 
a memory, 

a processing unit, 
and a computer program stored in the memory for causing the 
20 apparatus to carry out the steps of : 

storing in the memory position and velocity parameters 
defining an initial state of a model system having a plurality 
of bodies, 
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storing in the memory parameters defining at least one 
constraint function constraining the motion of the bodies in 
the model system 

storing in the memory parameters defining a bounded 
constraint force to simulate the effects of friction in which 
the constraint force acts in the plane of contact between a 
pair of objects to prevent sliding of one of the pair of 
objects over the other of the pair, wherein the constraint 
force is bounded to be not greater than a predetermined 
constant value to allow sliding of the objects over one another 
and thus include dynamic friction in the simulation, and 

calculating in the processor the position and velocity 
parameters defining the state of the system after a 
predetermined time step based on rigid body dynamics, including 

carrying out a semi-implicit integration step subject to 
the constraints, to determine the velocity after the step. 

33. A computer program recorded on a data carrier for 
simulating the motion of objects and displaying the results on 
a display screen, the computer program being operable to 
control a computer having a display screen, a memory and a 
processing unit to carry out the steps of 

storing in the memory position and velocity parameters 
defining an initial state of a model system having a plurality 
of bodies, 
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storing in the memory parameters defining at least one 
constraint function constraining the motion of the bodies in 
the model system, and 

calculating in the processor the position and velocity 
parameters defining the state of the system after a 
predetermined time step based on rigid body dynamics, including 

carrying out a semi -implicit integration step by 

calculating the velocity parameters after the time 

step from the external forces, the constraint forces and 

the position and velocity parameters before the time step, 

and 

calculating the position parameters after the time 
step from the external forces and constraint forces, the 
calculated velocity parameters after the time step and the 
position parameters before the time step; 
subject to the constraints, to determine the velocity 
after the step, including 

determining the constraint forces that act to keep the 
system in compliance with the constraints by ensuring that the 
first derivative of the constraint function is zero. 

34 . A computer program recorded on a data carrier for 
simulating the motion of objects and displaying the results on 
a display screen, the computer program being operable to 
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control a computer having a display screen, a memory and a 
processing unit to carry out the steps of 

storing in the memory position and velocity parameters 
defining an initial state of a model system having a plurality 
of bodies, the model including a model of friction in which the 
frictional force between a pair of objects is independent of 
the normal force between the objects; 

storing in the memory parameters defining at least one 
constraint function constraining the motion of the bodies in 
the model system, and 

calculating in the processor the position and velocity 
parameters defining the state of the system after a 
predetermined time step based on rigid body dynamics, including 
carrying out a semi -implicit integration step by 

calculating the velocity parameters after the time 
step from the external forces, the constraint forces and 
the position and velocity parameters before the time step, 
and 

calculating the position parameters after the time 
step from the external forces and constraint forces, the 
calculated velocity parameters after the time step and the 
position parameters before the time step; 
subject to the constraints, to determine the velocity 
after the step, including 
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determining the constraint forces that act to keep the 
system in compliance with the constraints by ensuring that the 
first derivative of the constraint function is zero. 

5 35. A computer program recorded on a data carrier for 

simulating the motion of objects and displaying the results on 
a display screen substantially as herein described with 
reference to the accompanying drawings . 

10 36. A method for operating a computer having a display 

screen, a memory and a processing unit for simulating the 
motion of objects and displaying the results on the display 
screen substantially as herein described with reference to the 
accompanying drawings . 

15 

37. Apparatus for simulating the motion of objects and 
displaying the results on a display screen comprising 
substantially as herein described with reference to the 
accompanying drawings . 

20 



25 
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/ Initialyze 
/ /-0 
< a(') = a (0) 

\ P(') = {l,2,...,/ J }\ a (0) 




Solve: 

= -<7a(0 
= z (0 + w (0 



/ 0 

V 



yes 



■s r > ov/ >— — — p> 



no 




Find smallest j 
such that 




yes 



a('+0 


= a(')u{ 7 } 


pO+i) 


= P<°\{/} 



a(' +| ) = a(')\{y> 



fig. J 



A 



/'<-/+! 
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Initialyze 
;' = 0 

a(0= a (P) 

('') = {l,2 ) ...,«}\ a (0) 

y(:)-{) 



1 




Solve: 
(psi,Y) 



5;= < 



mm(zj-l jt uj-zj) yyea 

(A/y a 2a - Myy/y - My^ + ?y)y VjEf 




yes 



■o 



Done 



Find smallest y 
such that 

sf<0 



1 



If/eaO andzj 0 </ y thena('' +1 )=a(')\{y} > y('+i) = y(0 U { y } l (,+i) = l (o 
IfyeaW mdzf > Uj a( ,+1 )= a (0\{;} > l (/+l) = l (/) U { y } y (, + i) =y (,) 
IfyeyO) a(' + » = a»u{y},y(' + i)= T (0 \{y} > l ('+D = l (0 
If y e 1 CO a (' +1 ) = a« u {y}, 1 <' + » = t © \ {y}, y (<+D = y (0 



fig. 3 
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Construct constraint geometry: 
1 -non-penetration: multiplier = X/ 
2-0 tangential velocity: multipliers = p/. 
j labels directions in contact plane 1 



Estimate normal 
force at each point 

5l/°> * 



Using boxed LCP 
algorithm, solve for 
LCP with: 




Satisfied? 



no 



yes 



Done 



Fig. 6 
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Body data array 
(includes M) 



© 7 



Constraint data 
array 



Interrogate constraints to 
create list of Jacobians 



calculate rhs 



tmp = M^fe 
tmp+ = vel/n 
rhs = J* tmp 
rhs=c/h- y/h 2 - J*tmp 



calculate A 



Calculate Problem Matrix 



feis 
resultant 
force on 
body 



where M is mass 
matrix of bodies, J 
is Jacobian matrix 



A (symmetric) upper triangle 



Copy upper triangle to lower, 
modify diagonal of A, add 
rotational force to bodies 



A matrix, now complete, 
with modified diagonal 



factorize 

- create A' 1 

- Cholesky 



IN:rhs, lo,hi,A, A" 1 
rhs becomes X 



J 

Calculate resultant forces 
from X and J 



J 

INTEGRATOR 
update velocities then 
positions & rotations 



u 

OUTPUT DATA: 
velocity (inc. angular velocity), force, 
position, orientation, for each body 
transformation matrix & orientation 
quaternion for each body 
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